#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>

#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"
#include "geometry_msgs/PoseStamped.h"
namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class VelodynePlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: VelodynePlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
      // Safety check
      if (_model->GetJointCount() == 0)
      {
        std::cerr << "Invalid joint count, Velodyne plugin not loaded\n";
        return;
      }
      else
        std::cerr << "\nThe velodyne plugin is attach to model /////////////////////[" <<
          _model->GetName() << "]\n";

      // Store the model pointer for convenience.
      this->model = _model;



      // Get the first joint. We are making an assumption about the model
      // having one joint that is the rotational joint.
      this->joint = _model->GetJoints()[6];

      // Setup a P-controller, with a gain of 0.1.
      this->pid = common::PID(0.1, 0, 0);

      // Apply the P-controller to the joint.
      this->model->GetJointController()->SetVelocityPID(
          this->joint->GetScopedName(), this->pid);

      double position_p = 0;
      if (_sdf->HasElement("position_p"))
      {
        position_p = _sdf->Get<double>("position_p");
      }

      double position_i = 0.1;
      if (_sdf->HasElement("position_i"))
        position_i = _sdf->Get<double>("position_i");

      double position_d = 0;
      if (_sdf->HasElement("position_d"))
        position_d = _sdf->Get<double>("position_d");

      ROS_INFO("position pid %f %f %f",position_p,position_i,position_d);
      common::PID pid_position = common::PID(position_p, position_i, position_d);

      this->model->GetJointController()->SetPositionPID(
          this->joint->GetScopedName(), pid_position);
      
      if (_sdf->HasElement("tor"))
        tor_ = _sdf->Get<double>("tor");
      if (_sdf->HasElement("upper"))
        upper_ = _sdf->Get<double>("upper");
      if (_sdf->HasElement("lower"))
        lower_ = _sdf->Get<double>("lower");

      // Set the joint's target velocity. This target velocity is just
      // for demonstration purposes.
      this->model->GetJointController()->SetVelocityTarget(
          this->joint->GetScopedName(), 0.0);


      this->model->GetJointController()->SetPositionTarget(
            this->joint->GetScopedName(), 0.0);


      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
        int argc = 0;
        char **argv = NULL;
        ros::init(argc, argv, "gazebo_client",
            ros::init_options::NoSigintHandler);
      }

      // Create our ROS node. This acts in a similar manner to
      // the Gazebo node
      this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so =
        ros::SubscribeOptions::create<std_msgs::Float32>("/" + this->model->GetName() + "/gimbal_yaw_vel_cmd",1,boost::bind(&VelodynePlugin::OnRosMsg, this, _1),
            ros::VoidPtr(), &this->rosQueue);
      this->rosSub = this->rosNode->subscribe(so);

      

      this->rosPub = this->rosNode->advertise<geometry_msgs::PoseStamped>("/" + this->model->GetName()+"/gimbal_yaw_angle", 1);


      // Spin up the queue helper thread.
      this->rosQueueThread =
        std::thread(std::bind(&VelodynePlugin::QueueThread, this));

    }

    /// \brief Handle an incoming message from ROS
    /// \param[in] _msg A float value that is used to set the velocity
    /// of the Velodyne.
    public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
    {
      double temp_cmd = _msg->data;
      if ((lim_upper && temp_cmd>0)||(lim_lower && temp_cmd<0)){
        temp_cmd = 0.0;
      }
        this->SetVelocity(_msg->data);
      // this->SetPosition(_msg->data);
    }

    /// \brief ROS helper function that processes messages
    private: void QueueThread()
    {
      static const double timeout = 0.01;

      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));

        geometry_msgs::PoseStamped msg;
        msg.header.stamp=ros::Time::now();
        double angle=this->joint->Position(0);

        if (angle > upper_ - tor_){
          lim_upper = true;
        }
        else if (angle < lower_ + tor_){
          lim_lower = true;
        }
        else{
          lim_upper = false;
          lim_lower = false;
          enable_control = true;
        }
        if ((lim_lower || lim_upper) && enable_control){
          this->SetVelocity(0.0);
          enable_control = false;
        }

          msg.pose.position.x = angle;
		    msg.header.frame_id=this->model->GetName();
        this->rosPub.publish(msg);

      }
    }



    /// \brief Set the velocity of the Velodyne
    /// \param[in] _vel New target velocity
    public: void SetVelocity(const double &_vel)
    {
      // Set the joint's target velocity.
      this->model->GetJointController()->SetVelocityTarget(
          this->joint->GetScopedName(), _vel);
    }      
    public: void SetPosition(const double &_position)
    {
        this->model->GetJointController()->SetPositionTarget(
                  this->joint->GetScopedName(), _position);
    }


    public: 
      double tor_ = 0.001;
      double upper_ = 2.617;
      double lower_ = -2.617;
      bool lim_upper = false;
      bool lim_lower = false;
      bool enable_control = true;

    /// \brief Pointer to the model.
    private: physics::ModelPtr model;

    /// \brief Pointer to the joint.
    private: physics::JointPtr joint;
    private: std::string only_joint_;

    /// \brief A PID controller for the joint.
    private: common::PID pid;


    /// \brief A node use for ROS transport
    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;

    private: ros::Publisher rosPub;

    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;

    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;




  };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(VelodynePlugin)
}
#endif
